#ifndef APP_PID_H_
#define APP_PID_H_


#include "system.h"

#define Pid_TASK_PRIO     11
#define Pid_task_STK_SIZE 1024

/*PID计算状态机定义*/
typedef enum
{
    APP_PID_INIT = 0,  ///< 初始化状态（配置PID参数）
    APP_PID_RUN,       ///< 运行状态（执行实时控制）
    APP_PID_STOP       ///< 停止状态（当前未实现）
} app_pid_status_t;

//PID控制器使能状态枚举
typedef enum {
    PID_DISABLE,   ///< 关闭PID计算
    PID_ENABLE,    ///< 启用PID计算
} positional_pid_status;

//四个电机的标签值
typedef enum
{
  A = 0,
  B,
  C,
  D,
} label;

//电机速度控制相关参数结构体
//在这个结构体里，由于使用了面向对象的编程思想，暴露了.c文件中其它函数的接口，并且其里面有它自己本身这个结构体，所以需要使用两个结构体名称_positional_pid_params_t，Motor_parameter
typedef struct _positional_pid_params_t
{
    int32_t Encoder;
    float Motor_Pwm;   //电机PWM数值，控制电机实时速度，即经过PID计算后得到的PWM值
    float Measure;     //实际测量值
    float Target;      //电机目标速度值，控制电机目标速度
    float Velocity_KP; //速度控制PID参数
    float Velocity_KI; //速度控制PID参数
    float Velocity_KD; //速度控制PID参数

    /*------------------------ 控制状态 ------------------------*/
    char control;  //（PID_ENABLE/PID_DISABLE）

    /*----------------------- 误差计算 -----------------------*/
    float error;        //< 当前误差
    float last_error;   //< 上一次误差（用于微分项计算）
    float prev_error;   //前一次的误差
    float error_Int;    //误差积分累加

    /*---------------------- 分量输出 ----------------------*/
    float p_out;    //< 比例项输出值
    float i_out;    //< 积分项输出值
    float d_out;    //< 微分项输出值

    /*---------------------- 总输出 ----------------------*/
    float output;        //< 最终输出值（经过限幅处理）
    float output_max;    //< 输出上限（抗积分饱和）
    float output_min;    //< 输出下限

    /*---------------------- 积分处理参数 ----------------------*/
    float separation_threshold;  //< 积分分离阈值（误差超过此值时关闭积分）
    float integral_limit_max;     //< 积分项上限
    float integral_limit_min;     //< 积分项下限

    /*死区范围*/
    float dead_zone;

    /*---------------------- 方法函数指针 ----------------------*/
        /*类似于python的面向对象编程*/
        /*C语言的结构体（struct）​只能包含数据成员（变量）和函数指针，不能直接定义函数实现。这是C语言与C++等面向对象语言的核心区别之一*/
        /*函数指针是 指向函数的指针变量，它本质上是一个变量，可以存储函数的地址，从而通过这个指针来调用函数*/
        /*例如，一个指向如下函数的函数指针：
        int add(int a, int b)
        {
                return a + b;
        }
                定义并使用函数指针：
        int (*func_ptr)(int, int);  // 声明一个函数指针
        func_ptr = add;             // 把函数地址赋值给指针
        int result = func_ptr(3, 4); // 通过函数指针调用函数，相当于：add(3, 4)*/

    // PID参数初始化接口
    void (*positional_pid_params_init)(
        struct _positional_pid_params_t *positional_pid,//在结构体中调用结构体的操作，必须给它起两个别名
        float kp, float ki, float kd,        // 基础系数
        float dead_zone,                    // 死区范围
        float output_max, float output_min  // 输出限幅
    );

    // 动态参数修改接口
    void (*positional_pid_set_value)(
        struct _positional_pid_params_t *positional_pid,
        float kp, float ki, float kd,              // PID系数
        float separation_threshold,                // 积分分离阈值
        float integral_limit_max,                  // 积分限幅
        float integral_limit_min
    );
    // 控制器使能状态切换接口
    void (*positional_pid_control)(
        struct _positional_pid_params_t *positional_pid,
        positional_pid_status status  // 控制状态
    );

}Motor_parameter;

/*PID计算函数*/
void Pid_task(void *parameter);

///* 控制PID的使能状态 */
//void positional_pid_control(Motor_parameter *positional_pid, positional_pid_status _status);

//位置式
float Moter_pid_compute(Motor_parameter *Moter_pid, float _target, rt_int32_t _measure);

//速度式
float Moter_pid_compute_V(Motor_parameter *Moter_pid, float _target, float _measure);

/* 初始化位置式PID控制器参数 */
void positional_pid_init(Motor_parameter *positional_pid, float kp,
                         float ki, float kd, float dead_zone, float output_max,
                         float output_min);

/*设置基准速度*/
void set_base_speed(rt_int32_t _speed);

// 激活车四个电机摄像头位置PID
void active_K230_position_pid(void);
// 停用四个电机摄像头位置PID
void deactive_K230_position_pid(void);
// 激活车角度环PID
void active_angle_pid(void);
// 停用车角度环PID
void deactive_angle_pid(void);
// 激活车四个电机编码器位置PID
void active_encoder_position_pid(void);
// 停用四个电机编码器位置PID
void deactive_encoder_position_pid(void);
// 激活车四个电机的麦轮全向运动PID
void active_mai_run_pid(void);
// 停用四个电机的麦轮全向运动PID
void deactive_mai_run_pid(void);
// 激活车四个电机摄像头机械臂抓取PID
void active_K230_catch_pid(void);
// 停用四个电机摄像头机械臂抓取PID
void deactive_K230_catch_pid(void);
//车自转
void Circle_Move(rt_int32_t speed);

void pid_reset(Motor_parameter *pid);

// 激活速度环
void activate_PID_SUDO(void);
// 停用四个电机的麦轮全向运动PID
void deactivate_PID_SUDO(void);

extern Motor_parameter MOTOR_ve[4];

#endif /* HARDWARE_BALANCE_H_ */
